#include "gripper_control/gripper_communicator.hpp"
#include "gripper_control/gripper_json.hpp"
#include <iostream>

namespace gripper_control
{
    GripperCommunicator::GripperCommunicator(const rclcpp::NodeOptions& options) 
        : Node("gripper_communicator", options),
        has_local_reached_(false),
        operate_command_(0),
        last_operate_command_(0),
        gripper_current_state_(0),
        has_gripper_got_command_(false),
         waiting_for_response_(false),
        gripper_initial_state_(0),
        error_code_(0),
        operation_completed_(false)
        
    {
        // 声明参数
        mqtt_feedback_topic_ = this->declare_parameter<std::string>(
            "mqtt_to_ros.feedback", "/fangshe/gripper/feedback"
        );
        mqtt_command_topic_ = this->declare_parameter<std::string>(
            "ros_to_mqtt.command", "/fangshe/gripper/command"
        );
        error_topic_ = this->declare_parameter<std::string>(
            "ros_to_mqtt.error", "/fangshe/gripper/error"
        );
        local_reached_topic_ = this->declare_parameter<std::string>(
            "local_reached_topic", "/fangshe/navigation/reached"
        );
        gripper_completion_topic_ = this->declare_parameter<std::string>(
            "gripper_completion_topic", "/fangshe/gripper/completed"
        );
        operate_command_ = this->declare_parameter<int>(
            "operate_command", 0
        );

        source_type_ = this->declare_parameter<std::string>(
            "source_type", "default"
        );
        action_type_ = this->declare_parameter<std::string>(
            "action_type", "default"
        );


        // 初始化映射表
        initializeCommandMap();

        // 根据初始化参数设置操作命令
        updateOperateCommand();


        // 创建订阅者和发布者
        mqtt_feedback_sub_ = this->create_subscription<std_msgs::msg::String>(
            mqtt_feedback_topic_, 10,
            std::bind(&GripperCommunicator::mqttFeedbackCallback, this, std::placeholders:: _1)
        );
        local_reached_sub_ = this->create_subscription<std_msgs::msg::Bool>(
            local_reached_topic_, 10,
            std::bind(&GripperCommunicator::localReachingCallback, this, std::placeholders:: _1)
        );
        
        mqtt_command_pub_ = this->create_publisher<std_msgs::msg::String>(
            mqtt_command_topic_, 10
        );
        error_pub_ = this->create_publisher<std_msgs::msg::String>(
            error_topic_, 10
        );
        completion_pub_ = this->create_publisher<std_msgs::msg::Bool>(
            gripper_completion_topic_, rclcpp::QoS(1).transient_local()
        );

        RCLCPP_INFO(this->get_logger(), "Gripper communicator ready.");
        RCLCPP_INFO(this->get_logger(), "Initial source_type: %s, action_type: %s, operate_command: %d",
                    source_type_.c_str(), action_type_.c_str(), operate_command_);

    }


    void GripperCommunicator::initializeCommandMap(){
        // 初始化映射表
        command_map_ = {
            // {std::make_pair(source_type, action_type), operate_command}
            {std::make_pair("default", "default"), 0},
            {std::make_pair("default", "grasp"), 0},
            {std::make_pair("default", "release"), 0},

            {std::make_pair("neutron", "release"), 1},
            {std::make_pair("neutron", "grasp"), 2},
            {std::make_pair("gamma", "release"), 3},
            {std::make_pair("gamma", "grasp"), 4},

            // 测试专用
            {std::make_pair("x", "x"), 12},
        };

        RCLCPP_INFO(this->get_logger(), "Command map initialized with %zu entries", command_map_.size());
    }


    void GripperCommunicator::updateOperateCommand(){
        auto key = std::make_pair(source_type_, action_type_);
        auto it = command_map_.find(key);

        if(it != command_map_.end()){
            operate_command_ = it->second;
        }
        else{
            // 若没有映射到对应的命令，则使用默认的命令
            operate_command_ = 0;
            RCLCPP_ERROR(this->get_logger(), "Invalid source_type or action_type");
        }
    }
    
    void GripperCommunicator::localReachingCallback(const std_msgs::msg::Bool::SharedPtr msg){ 
        bool previous_reached_status = has_local_reached_;
        has_local_reached_ = msg->data;

        // 若导航尚未到达，直接退出本次循环
        if(!has_local_reached_){
            return;
        }

        // 发布抓钩操作指令
        if(!previous_reached_status && has_local_reached_){
            sendCommand();
            RCLCPP_INFO(this->get_logger(), "Local reached: %s", has_local_reached_ ? "true" : "false");
        }
    }

    // 抓钩反馈回调函数
    void GripperCommunicator::mqttFeedbackCallback(const std_msgs::msg::String::SharedPtr msg){
        // 解析JSON
        if(feedback_json_.parse_json_string(msg->data)){
            // 提取抓沟当前正在处理的命令
            gripper_current_state_ = feedback_json_.get_zhua_cur_cmd();
        }
    }

    void GripperCommunicator::sendCommand(){ 

        // has_local_reached_为true后开始发布命令，指令内容暂时从参数中获取
        command_json_.set_from_command(operate_command_);
        std::string json_string = command_json_.get_cmd_json_string();

        if(!json_string.empty() && json_string != "null") {
            if(operate_command_ != last_operate_command_){
                std_msgs::msg::String msg;
                msg.data = json_string;
                mqtt_command_pub_->publish(msg);
                RCLCPP_INFO(this->get_logger(), "Send command: %s", json_string.c_str());

                last_operate_command_ = operate_command_;
            }
        }

        // 开始启动5s的响应等待
        startWaiting();
    }


    void GripperCommunicator::startWaiting(){ 
        gripper_initial_state_ = 0;
        // 开启开始等待标志位
        waiting_for_response_ = true;
        // 记录当前开始等待时刻
        command_time_ = this->now();

        // 启动响应检查定时器（5s）
        response_timer_ = this->create_wall_timer(
            std::chrono::milliseconds(100),
            std::bind(&GripperCommunicator::checkResponse, this)
        );
    }

    void GripperCommunicator::checkResponse(){ 
        if(waiting_for_response_ && (gripper_current_state_ == operate_command_) &&
            (this->now() - command_time_ < std::chrono::seconds(5))){
            has_gripper_got_command_ = true;
            waiting_for_response_ = false;

            // 取消响应定时器
            if(response_timer_) {
                response_timer_->cancel();
            }

            // 启动任务完成检查定时器
            completion_timer_ = this->create_wall_timer(
            std::chrono::milliseconds(100),
            std::bind(&GripperCommunicator::checkCompletion, this));

        }
        else if(waiting_for_response_ && (this->now() - command_time_ > std::chrono::seconds(5))){
            // 响应超时
            has_gripper_got_command_ = false;
            waiting_for_response_ = false;
            // 取消响应定时器
            if(response_timer_) {
                response_timer_->cancel();
            }
            publishError();
        }
    }

    void GripperCommunicator::checkCompletion(){ 
        // 抓手命令接收标志位和抓手状态一致性成功表示抓取成功
        if(has_gripper_got_command_ && (gripper_current_state_ == 0)){
            operation_completed_ = true;
            has_gripper_got_command_ = false;
            operate_command_ = 0;

            // 取消完成检查定时器
            if(completion_timer_) {
                completion_timer_->cancel();
            }

            publishCompletion();
        }

        // 注意：这里不应该在其他情况下都发布错误，因为可能还在执行中
        // 只有在超时或其他明确错误情况下才发布错误
    }


    // 发布任务完成
    void GripperCommunicator::publishCompletion(){
        std_msgs::msg::Bool msg;
        msg.data = true;
        completion_pub_->publish(msg);
        RCLCPP_INFO(this->get_logger(), "Gripper operation completed.");
    }

    void GripperCommunicator::publishError(){ 
        RCLCPP_ERROR(this->get_logger(), "Gripper command timeout.");
        error_json_.set_from_error(1);
        std_msgs::msg::String msg;
        msg.data = error_json_.get_error_json_string();
        error_pub_->publish(msg);
    }


} // namespace gripper_control


int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<gripper_control::GripperCommunicator>());
    rclcpp::shutdown();
    return 0;
}